import logging
import unittest

from Model import *
from Model2 import *
from Model3 import *
from History import *

class CreateModel(unittest.TestCase):
    
    def setUp(self):
        pass

    def testGetControls(self):
        model = Model()
        model.k1 = 1
        model.k2 = 1
        model.k3 = 1
        model.k4 = 1
        
        #model.printMe()
        
        dt = 0.1
        a = 0.4  # Target angle
        aDot = 0 # Target angular velocity
        #print model.getControls(dt, a, aDot)

    def testSetParameters(self):
        model = Model3()
        
        events = []
        
        s0 = State()
        s0.arm1.a = 0
        s0.arm1.aDot = 0.1
        s0.arm1.c1 = 1
        s0.arm1.c2 = 3
        e0 = Event(s0, 0.0)
        events.append(e0)
        s1 = State()
        s1.arm1.a = 0.07
        s1.arm1.aDot = 0.3
        s1.arm1.c1 = 1
        s1.arm1.c2 = 3
        e1 = Event(s1, 0.1)
        events.append(e1)
        s2 = State()
        s2.arm1.a = 0.2
        s2.arm1.aDot = 0.5
        e2 = Event(s2, 0.2)
        events.append(e2)
        
        model.setParameters(events)
        
        # Try to go from state 0 to state 1 and see what the controls should be
        stiffness = 0.5*(s0.arm1.c1+s0.arm1.c2) - math.fabs(s0.arm1.c1-s0.arm1.c2)
        print stiffness
        print model.getControls(0.1, s0.arm1.a, s0.arm1.aDot, s1.arm1.a, s1.arm1.aDot, 0)
        
#    def testSetParameters3(self):
#        history = loadHistory("history.pickle")
#        model = Model2()
#        e1 = history.events[10]
#        e2 = history.events[11]
#        e3 = history.events[12]
#        model.setParameters(history.events[0:3])
#        model.test(history.events[0:3])
        
        
#logging.basicConfig(level=logging.DEBUG, format='%(asctime)s %(levelname)s: %(message)s', datefmt='%H:%M:%S')

if __name__ == "__main__":
    unittest.main()   